#pragma config(Sensor, dgtl1,  EncoderR,            sensorQuadEncoder)
#pragma config(Sensor, dgtl3,  EncoderL,            sensorQuadEncoder)
#pragma config(Motor,  port1,           LeftLift1,     tmotorNormal, openLoop)
#pragma config(Motor,  port3,           DriveL,        tmotorNormal, openLoop)
#pragma config(Motor,  port4,           DriveR,        tmotorNormal, openLoop, reversed)
#pragma config(Motor,  port6,           Gate1,         tmotorNormal, openLoop)
#pragma config(Motor,  port7,           Gate2,         tmotorNormal, openLoop)
#pragma config(Motor,  port10,          LeftLift2,     tmotorNormal, openLoop, reversed)
//*!!Code automatically generated by 'ROBOTC' configuration wizard               !!*//

int max(int original) {
  if(original > 127)
    return 127;
  if(original < -127)
    return -127;
  else
    return original;
}

int trim (int input) {
  if(abs(input) < 15)
    return 0;
  else
    return input;
}

/* DRIVE FUNCTIONS
 * drive_forward_1msec
 * drive_straight
 * drive_straight_to_touch
 */

 /* drive_forward_msec
  * @purpose: autonomously drive robot forward for provided duration
  * @param speed: speed of motors, -127 to 127; negative values go reverse
  * @param duration: duration in milliseconds
 */

void drive_forward_msec(int speed, int duration) {
  motor[DriveR] = speed;
  motor[DriveL] = speed;
  wait1Msec(duration);
  motor[DriveR] = 0;
  motor[DriveL] = 0;
}
/* drive_straight
  * @purpose: autonomously drive robot forward for provided distance in inches
  * precondition: 4 inch wheels used; if other wheels, change the 4 in the while loop
  * @param speed: speed of motors, -127 to 127; negative values go reverse
  * @param distance: distance in inches
  * TODO: consider adding an acceleration loop
 *///there are 360 ticks in an encoder revolution
void drive_straight(int speed, float distance) {
  SensorValue[EncoderL] = 0;
  SensorValue[EncoderR] = 0;
  while( ( (float) SensorValue[EncoderL]  * 4 / 360 * PI) < distance &&
         ( (float) SensorValue[EncoderR] * 4 / 360 * PI) < distance) {
    if(SensorValue[EncoderL] > SensorValue[EncoderR]) {
      motor[DriveR] = speed;
      motor[DriveL]  = speed - 1;
    }
    else if(SensorValue[EncoderL] < SensorValue[EncoderR]) {
      motor[DriveR] = speed - 1;
      motor[DriveL]  = speed;
    }
    else {
      motor[DriveR] = speed;
      motor[DriveL]  = speed;
    }
  }
  motor[DriveL] = 0;
  motor[DriveR] = 0;
}

/* drive_straight_unlocked
  * @purpose: autonomously drive robot forward for provided distance in inches WHILE allowing
  * precondition: 4 inch wheels used; if other wheels, change the 4 in the while loop
  * @param speed: speed of motors, -127 to 127; negative values go reverse
  * @param distance: distance in inches
  * TODO: consider adding an acceleration loop
 *///there are 360 ticks in an encoder revolution
void drive_straight_unlocked(int speed, float distance) {
  SensorValue[EncoderL] = 0;
  SensorValue[EncoderR] = 0;
  if( ( (float) abs(SensorValue[EncoderL])  * 2 / 360 * PI) < distance &&
      ( (float) abs(SensorValue[EncoderR]) * 2 / 360 * PI) < distance) {
    if(abs(SensorValue[EncoderL]) > abs(SensorValue[EncoderR])) {
      motor[DriveR] = -speed;
      motor[DriveL]  = -speed + 1;
    }
    else if(abs(SensorValue[EncoderL]) < abs(SensorValue[EncoderR])) {
      motor[DriveR] = -speed + 1;
      motor[DriveL]  = -speed;
    }
    else {
      motor[DriveR] = -speed;
      motor[DriveL]  = -speed;
    }
  }
}

/* drive_straight_to_touch
  * @purpose: autonomously drive robot forward until touch sensors are triggered
  * precondition: 2 touch sensors, left and right
  * @param speed: speed of motors, -127 to 127; negative values go reverse
 */
/*void drive_straight_to_touch(int speed) {
  while(SensorValue[LeftTouch]  == 0 || SensorValue[RightTouch] == 0) {
    if(SensorValue[EncoderL] > SensorValue[EncoderR]) {
      motor[DriveR] = speed;
      motor[DriveL]  = speed - 1;
    }
    else if(SensorValue[EncoderL] < SensorValue[EncoderR]) {
      motor[DriveR] = speed - 1;
      motor[DriveL]  = speed;
    }
    else {
      motor[DriveR] = speed;
      motor[DriveL]  = speed;
    }
  }
  motor[DriveL] = 0;
  motor[DriveR]= 0;
}*/

/* turn
  * @purpose: autonomously execute a point turn
  * @param speed: speed of motors, 0 to 127; no negative values
  * @param degrees: number of degrees to turn
  * TODO: complete this function
 */
void turn(int speed, int degrees) {
  SensorValue[EncoderL] = 0;
  SensorValue[EncoderR] = 0;
  while(SensorValue[EncoderL] < degrees || SensorValue[EncoderR] < degrees*2) {
    motor[DriveL] = speed;
    motor[DriveR] = -speed;
  }
  motor[DriveL] = 0;
  motor[DriveR] = 0;
}

void gate(int speed) {
  motor[Gate1] = speed;
  motor[Gate2] = speed;
  wait1Msec(2000);
  motor[Gate1] = 0;
  motor[Gate2] = 0;
}

void lift(int speed) {
  motor[LeftLift1] = speed;
  motor[LeftLift2] = speed;
  wait1Msec(2000);
  motor[LeftLift1] = 0;
  motor[LeftLift2] = 0;
}

/*void turn_msec(int speed, int duration) {

  if(){
    motor[DriveL]=-speed;
    motor[DriveR]=speed;
    wait1Msec(duration);
    motor[DriveL]=0;
    motor[DriveR]=0;}
  else{
    motor[DriveL]=speed;
    motor[DriveR]=-speed;
    wait1Msec(duration);
    motor[DriveL]=0;
    motor[DriveR]=0;}
}*/

/* MODE SELECTION FUNCTION
 * mode_select
 * @purpose use a potentiometer to set mode
 * high potentiometer values return 1, low potentiometer values return 0
 */

/*int mode_select() {
  if(SensorValue[Mode] >= 2000)
    return 1;
  else
    return 0;
}*/


/* LINE FOLLOWER FUNCTIONS
 * follow_line_msec
 * follow_line_distance
 */

 /*follow_line_msec
 * @purpose follow a line for provided duration using 3 light sensors
 * @param speed: speed of motors, 0 to 127
 * @param duration: duration of line following in milliseconds
 */
/*void follow_line_msec(int speed, int duration) {
  ClearTimer(T1);
  int threshold = 250;  //guessed threshold; low numbers are lighter
  while(time1[T1] < duration) {
    if(SensorValue[MidLine] < threshold) {  //middle sensor is on line
     motor[DriveL]  = speed;
     motor[DriveR] = speed;
    }
    else if(SensorValue[LeftLine] < threshold) {  //left sensor is on line
      motor[DriveL]  = speed - 1;
      motor[DriveR] = speed;
    }
    else if(SensorValue[RightLine] < threshold) { //right sensor is on line
      motor[DriveL]  = speed;
      motor[DriveR] = speed - 1;
    }
  }
}*/


 /*follow_line_distance
 * @purpose follow a line for provided duration using 3 light sensors
 * precondition: 4-inch wheels
 * @param speed: speed of motors, 0 to 127
 * @param distance: distance to travel in inches
 */
/*void follow_line_distance(int speed, float distance) {
  int threshold = 250;  //guessed threshold; low numbers are lighter
  while(((float)SensorValue[EncoderL]+(float)SensorValue[EncoderR])/2 * 4 / 360 * PI
                                                                      < distance){
    if(SensorValue[MidLine] < threshold) {  //middle sensor is on line
     motor[DriveL]  = speed;
     motor[DriveR] = speed;
    }
    else if(SensorValue[LeftLine] < threshold) {  //left sensor is on line
      motor[DriveL]  = speed - 1;
      motor[DriveR] = speed;
    }
    else if(SensorValue[RightLine] < threshold) { //right sensor is on line
      motor[DriveL]  = speed;
      motor[DriveR] = speed - 1;
    }
  }
}*/
